#include "fkine.h"
#include "ikine.h"
#include "main.h"
#include "math.h"
#include "Robot.h"

fkine_t fkine;

void fkine_fun(fkine_t *kine)
{
    float mat[16];
    float q[6];

    for(uint8_t ii=0;ii<6;ii++){
        q[ii]=kine->q[ii];
    }

    mat[0]=cos(q[1]+q[2])*sin(q[4])+sin(q[1]+q[2])*cos(q[3])*cos(q[4]);
    mat[1]=cos(q[1]+q[2])*sin(q[4])-sin(q[1]+q[2])*cos(q[3])*cos(q[4]);
    mat[2]=sin(q[1]+q[2])*sin(q[3]);
    mat[3]=170*cos(q[1]+q[2])+190*sin(q[1]);

    mat[4]=-cos(q[4])*sin(q[3]);
    mat[5]=sin(q[3])*sin(q[4]);
    mat[6]=cos(q[3]);
    mat[7]=q[0]-1328.0f/5.0f;

    mat[8]=cos(q[1]+q[2])*cos(q[3])*cos(q[4])-sin(q[1]+q[2])*sin(q[4]);
    mat[9]=-sin(q[1]+q[2])*cos(q[4])-cos(q[1]+q[2])*cos(q[3])*sin(q[4]);
    mat[10]=cos(q[1]+q[2])*sin(q[3]);
    mat[11]=190*cos(q[1])-170*sin(q[1]+q[2])+157;

    mat[12]=0;
    mat[13]=0;
    mat[14]=0;
    mat[15]=1;

    for(uint8_t i=0;i<16;i++){
        kine->T[i]=mat[i];
    }
}

void J23_fkine(pose_t *fkine)
{
    float target_len_2,L1_2,L2_2;
    L1_2=L1*L1;
    L2_2=L2*L2;

    fkine->target_len= sqrtf(L1_2+L2_2-2*L1*L2*(cosf((180.0f-fkine->joint[3]+fkine->joint[2])*3.14f/180.0f)));
    target_len_2=fkine->target_len * fkine->target_len;
    fkine->target_angle=90-fkine->joint[2] - (acosf((L1_2+target_len_2-L2_2)/(2*L1*fkine->target_len))*180.0f/3.14f);

    fkine->Y= fkine->target_len * cosf(fkine->target_angle*3.14f/180.0f);
    fkine->Z= fkine->target_len * sinf(fkine->target_angle*3.14f/180.0f);
    fkine->X= fkine->joint[1]+55.6f;
    fkine->Roll=fkine->joint[4];
    fkine->Pitch=fkine->joint[5];
    fkine->Yaw=0;
}

